(proclaim '(optimize (speed 3)))


(defvar +bignum+ 1000000)

(defconstant pi/2 (/ pi 2))
(defconstant 3pi/2 (/ (* pi 3) 2))
(defconstant 2pi (* pi 2))

; map
;(defparameter *map* (make-hash-table :test #'equalp))
(defparameter *map* ())
(defvar *dx*)
(defvar *dy*)
(defparameter *ms* ())

; object format ((x . y) r), like: ((100 . 30) 0.95)
(defun update-map (objs)
  (remove-duplicates (append objs *map*) :test #'equalp))
;  (dolist (obj objs)
;    (unless (gethash (car obj) *map*)
;      (setf (gethash (car obj) *map*) (cadr obj)))))

; rover data
(defvar *max-turn*)
(defvar *max-speed*)
(define-symbol-macro *sec1* (* 10 *max-speed*)) ; a length of an etalon section (used to calculate arbitrary postions by directions

; rover position from the last telemetry reading
(defvar *x*) ; x
(defvar *y*) ; y
(define-symbol-macro *pos* (list *x* *y*))
(defvar *home* '(0 0))
(defvar *a*) ; angle
(defvar *t*) ; time

(proclaim '(inline update-pos))
(defun update-pos (x y)
  (setf *x* x *y* y))


; utilities
(proclaim '(inline rad deg cmd dist 2els calc-turn-r))

(defun cmd (dir a v)
  (princ (format nil "~a ~a ; " a v))
  (force-output *standard-output*))

(defun rad (deg)
  (/ (* deg pi) 180))

(defun deg (rad)
  (round (/ (* 180 rad) pi)))

(defun dist (pos1 pos2)
  (sqrt (+ (expt (- (car pos1) (car pos2)) 2)
	   (expt (- (cadr pos1) (cadr pos2)) 2))))

(defun 2els (lst &optional (start 0))
  (list (nth start lst) (nth (1+ start) lst)))

(defun calc-turn-r (v)
  (/ *max-deg* v))

; doesn't work
#|
(defun intersect (lst1 lst2 dir)
  (let ((start (car lst1))
	(finish (cadr lst1))
	(1st (car lst2))
	(2nd (cadr lst2)))
    (ecase dir
      (#\L (or (and (>= 1st start)
		    (<= 1st finish))
	       (and (>= 2nd start)
		    (<= 2nd finish))))
      (#\R (or ( (<= 1st start)
		    (>= 1st finish))
	       (and (>= 2nd start)
		    (<= 2nd finish)))))))
|#

;(defun logmsg (str)
;  (with-open-file (log "llog.txt" :direction :output :if-exists :append :if-does-not-exist :create)
;    (format log "~a: ~a~%" (get-universal-time) str))
;  str)

; program loop

(defun controller ()
  (with-output-to-string (data)
    (loop for ch = (handler-case
		       (read-char *standard-input* nil)
		     (error () #\;)) while ch
       do (write-char ch data)
       when (char= ch #\;) do (handler-case
				  (react (get-output-stream-string data))
				(error () nil))))) ;(logmsg (format nil "~a" e)))))))

(let ((counter 0))
  (defun react (str)
    (if (= counter 0)
	(progn
	  (setf counter 10)
	  (case (elt (string-left-trim '(#\Space) str) 0)
	    (#\I (destructuring-bind (dx dy ms mt) (process-telemetry str)
		   (setf *dx* dx *dy* dy *max-speed* ms *max-turn* mt)))
	    (#\T (destructuring-bind (turn-dir x y a v objs ms) (process-telemetry str)
		   (update-pos x y)
		   (update-map objs)
		   (if ms
		       (if (< (length ms) 3)
			   (issue-escape-dir turn-dir a v ms)
			   (issue-escape-dir turn-dir a v (calc-closest-ms ms)))
		       (issue-dir turn-dir a v))))))
	(decf counter)))) ; skip

(defun process-telemetry (str)
  (macrolet ((skip-rd (&optional (times 1))
	       `(dotimes (i ,times)
		  (read in)))
	        
	     (rd (&optional (times 1))
	       `(progn (skip-rd (1- ,times))
		       (read in))))

    (with-input-from-string (in str)
      (let ((type (read-char in nil)))
	(case type
	  (#\I (list (rd)		; dy
		     (rd)		; dx
		     (rd 4)		; max-speed
		     (rd 2)))		; max-hard-turn
	  (#\T (nconc (list (progn (skip-rd)
				   (read-char in)
				   (read-char in)) ; turn-char
			    (rd)		   ; x
			    (rd)		   ; y
			    (rd)                   ; a
			    (rd))                  ; v
		      (loop with os = () and ms = ()
			 do (let ((char (read-char in nil)))
			      (case char
				(#\Space nil)
				(#\; (return (list os ms)))
				(#\m (push (list #\m
						 (rd)  ; x
						 (rd)  ; y
						 (rd)  ; dir
						 (rd)) ; speed
					   ms))
				(t (push (list char  ; type
					       (rd)  ; x
					       (rd)  ; y
					       (rd)) ; r
					 os))))))))))))


;;; strategy A1

#|
(defun calc-free-sector-radius (deg1 deg2 dir)
  (let ((min +bignum+))
    (dolist (obj (remove #\b objs :key #'car))
      (let* ((pos (2els obj))
	     (dst (dist pos *pos*))
	     (r (nth 2 obj))
	     (d1 (rad deg1))
	     (d2 (rad deg2)))
	(when (intersect (list d1 d2)
			 (let ((d (atan (/ (expt (- (car pos) *x*) 2)
					   (expt (- (cadr pos) *y*) 2))))
			       (dd (atan (/ (dist pos *pos*) r)))) ; delta
			   (list (+ d dd)
				 (let ((d- (- d dd)))
				   (if (> d- 0) d- (+ 2pi d-)))))
			 dir)
	  (- dst r)))))) ; TODO: calc as for a circle
|#

(defun calc-dir (pos &optional (pos0 '(0.0 0.0)))
  "Calculates the best movement direction in terms of shortest path to the homebase with the currently available mapping information and no martians in sight and memory"
  (let* ((x (car pos))
	 (y (cadr pos))
	 (x0 (car pos0))
	 (y0 (cadr pos0))
	 (d (atan (abs (- y y0))
		  (abs (- x x0)))))
    (if (> x0 x)
	(if (> y0 y) d
	    (- 2pi d))
	(if (> y0 y) (- pi d)
	    (+ pi d)))))


(defparameter *rec-limit* 1)

(defun calc-dir-w-obstacles (pos dir-range rec-level)
  (let ((home-dir (calc-dir pos))
	(home-dst (dist pos *home*))
	(min-dst +bignum+)
	(closest-obj nil))
    ; find the closest obstacle
    (dolist (obj (remove-if-not #'(lambda (x) ; home-dir line and object circle intersect
				    (let ((tg (tan home-dir))
					  (a (cadr x))
					  (b (caddr x))
					  (r (cadddr x)))
				      (and (>= (expt (+ b (* tg a)) 2))
					       (* (1+ (expt tg 2))
						  (- (+ (expt a 2)
							(expt b 2))
						     (expt r 2)))))
				    (< (dist pos (2els x 1)) home-dst))
				*map*))
      (let ((cur-dst (- (dist pos (2els obj 1)) (cadddr obj))))
	(when (< cur-dst min-dst)
	  (setf min-dst cur-dst
		closest-obj obj))))
	       
    (if closest-obj
	; calculate distances by tangents to the left and right
	(let* ((obj-pos (2els closest-obj 1))
	       (r (+ (nth 3 closest-obj) 0.2))
	       (home-dir-rev (calc-dir *home* pos))

	       ; angles (hard to explain -- better to draw)...

	       ; from the rover side
	       (d (calc-dir pos obj-pos))
	       (dshift (abs (- home-dir d)))
	       (dd (asin (/ r (dist pos obj-pos))))
	       (dl (- dd dshift))
	       (dr (+ dd dshift))

	       ; from home-base side
	       (d0 (calc-dir *home* obj-pos))
	       (d0shift (abs (- home-dir-rev d0)))
	       (dd0 (asin (/ r (dist *home* obj-pos))))
	       (d0l (- dd0 d0shift))
	       (d0r (+ dd0 d0shift))
				   
	       ; calculate dists to intermediate points, which are situated on the intersection of tangents from current pos and home-base
	       (dst-l (/ (* home-dst (sin d0l))
			 (sin (- pi (+ dl d0l)))))
	       (dst-r (/ (* home-dst (sin d0r))
			 (sin (- pi (+ dr d0r)))))

	       ; angles correction for proper coordinate calculation
	       (ax (if (or (< d  pi/2)
			   (and (< d 3pi/2) (> d pi)))
		       d
		       (- d)))
	       (ay (if (or (< d pi/2)
			   (and (< d 3pi/2) (> d pi)))
		       d
		       (- pi d)))
				     
	       ; intermediate points' coords
	       (new-l-pos (list (+ (car pos)
				   (* dst-l (sin (- ax dd))))
				(+ (cadr pos)
				   (* dst-l (cos (- ay dd))))))
	       (new-r-pos (list (+ (car pos)
				   (* dst-r (cos (+ ax dd))))
				(+ (cadr pos)
				   (* dst-r (sin (+ ay dd))))))
	  
	       ; claculate dirs to intermediate points
	       (dir-l (calc-dir pos new-l-pos))
	       (dir-r (calc-dir pos new-r-pos)))

	  ; if we can, recurse
	  (when (< rec-level *rec-limit*)
	    (incf dst-l (calc-dir-w-obstacles new-l-pos dir-range (1+ rec-level)))
	    (incf dst-r (calc-dir-w-obstacles new-r-pos dir-range (1+ rec-level))))

	  ; choose the shortest of the 2 paths
	  (let ((weight-l (in-range dir-l dir-range))
		(weight-r (in-range dir-r dir-range)))
	    (macrolet ((values-l () `(values dst-l   ; distance
					     dir-l)) ; dir
		       (values-r () `(values dst-r
					     dir-r)))
	      (cond
		((and (= weight-l 0)	; both dirs are ok
		      (= weight-r 0)) (if (< dst-l dst-r)
					  (values-l)
					  (values-r)))
		 ((= weight-l 0) (values-l))
		 ((= weight-r 0) (values-r))
		 ; if all the paths are not ok, use the one, which is shorter and more ok (both terms have the same importance)
		 (t (if (< (/ dst-l weight-l) (/ dst-r weight-r))
			(values-l)
			(values-r)))))))

	; return the dir to home-base, if there's no obstacle
	(multiple-value-bind (weight dir) (in-range home-dir dir-range)
	  (if (= weight 0)
	      (values home-dst  ; distance
		      home-dir) ; dir
	      (values home-dst  ; fake distance, cause we won't need it
		      dir))))))

(defun in-range (dir range)
  (let* ((beg (car range))
	 (end (cdr range))
	 (fun (if (> end beg)
		 #'<
		 #'>)))
    (if (or (and (> end beg) ; clock-wise
		 (or (< dir beg) (> dir end)))
	    (and (< end beg) ; counter-clock-wise
		 (and (< dir beg) (> dir end)))
	    (= beg end))
	0 ; out-of-range
	; in range
	(let ((b (abs (- dir beg)))
	      (e (abs (- dir end))))
	  (if (< b e)
	      (values (/ b pi/2) ; weight
		      beg)       ; dir
	      (values (/ e pi/2)
		      end))))))

(defun issue-dir (turn-dir a v)
  (cmd turn-dir
       (deg (cadr (multiple-value-list (calc-dir-w-obstacles *pos* '(0 . 0) 1))))
       *max-speed*))


;;; strategy A2

(defun calc-closest-ms (ms)
  (2els (sort ms #'<
	      :key (lambda (x) (dist (2els x 1) *pos*)))))

(defun calc-escape-dir (m1 &optional m2)
  "Calculates the dir, we should choose to escape from 1 upto 2 martians.
Martians are supplied as a list of the form (x y a v). Only martians currently in sight are accounted for"
  (macrolet ((compute-rez ()
	       `(multiple-value-bind (weight dir) (in-range home-dir m-range)
		  (cadr (multiple-value-list
			 (calc-dir-w-obstacles (if (= weight 0)
						   *pos* ; marcian(s) do(es)n't bother us
						   ; search for path in the direction at least wpi/2 from marcian and as close to home-dir, as possible
						   (list (* *sec1* (cos dir))
							 (* *sec1* (sin dir))))
					       m-range
					       1)))))))

  (let ((home-dir (calc-dir *pos*)))
    (macrolet ((compute-span (m-dir wpi/2 dir)
		 `(ecase ,dir
		    ('down (if (< ,m-dir ,wpi/2)
			       (- 2pi (- ,wpi/2 ,m-dir))
			       (- ,m-dir ,wpi/2)))
		    ('up (if (> ,m-dir (- 2pi ,wpi/2))
			     (- ,m-dir (- 2pi ,wpi/2))
			     (+ ,m-dir ,wpi/2))))))

      (if (not m2)
	  (let* ((m-dir (calc-dir *pos* (2els m1)))
		 ; if marsian's speed is less, than ours, make it's range lesser (than pi), otherwise its pi
		 (m-weight (min 1 (/ (nth 3 m1)
				     *max-speed*)))
		 (wpi/2 (* pi/2 m-weight)) ; weighted pi/2, which is no greater, than pi/2
		 (m-range (cons (compute-span m-dir wpi/2 'down)
				(compute-span m-dir wpi/2 'up))))
	    (compute))

	  (let* ((m1-dir (calc-dir *pos* (2els m1)))
		 (m1-weight (min 1 (/ (nth 3 m1)
				      *max-speed*)))
		 (m2-dir (calc-dir *pos* (2els m2)))
		 (m2-weight (min 1 (/ (nth 3 m2)
				      *max-speed*)))
		 (w1pi/2 (* pi/2 m1-weight))
		 (w2pi/2 (* pi/2 m2-weight))

		 (m1down (compute-span m1-dir w1pi/2 'down))
		 (m1up   (compute-span m1-dir w1pi/2 'up))
		 (m2down (compute-span m2-dir w2pi/2 'down))
		 (m2up   (compute-span m2-dir w2pi/2 'up))

		 (m-range (cons (if (or (< m1down m2down)
					(and (> m1down 3pi/2)
					     (< m2down pi/2)))
				    m1down
				    m2down)
				(if (or (> m1up m2up)
					(and (< m1up pi/2)
					     (> m2up 3pi/2)))
				    m1up
				    m2up))))
	    (compute-rez))))))

(defun issue-escape-dir (turn-dir a v ms)
  (cmd turn-dir
       (deg (calc-escape-dir (car ms)))
       *max-speed*))